#include "kalman.h"
#include <Eigen/Eigen>

#ifdef _DEBUG
#include <iostream>
#endif

using namespace common;

KalmanFilter::KalmanFilter():
m_time(0.0)
{
	m_x.setZero();
	m_P.setZero();
	m_Q.setZero();
	m_R.setZero();
}

KalmanFilter::~KalmanFilter()
{
	/* void */
}

RectCoords KalmanFilter::getCoords(void) const
{
	return RectCoords(m_x[0], m_x[1]);
}

RectCoords KalmanFilter::getVel(void) const
{
	return RectCoords(m_x[2], m_x[3]);
}

void KalmanFilter::updateState(const RectCoords& a_measure, Time a_time)
{
	/// # Этапы обработки
	/// ## Экстраполяция
	/// Разница во времени:
	/// \f[\Delta_T = T_m - T\f]
	Time delta_T = a_time - m_time;
	m_time = a_time;

	/// \f$F\f$ -- матрица эволюции процесса\n
	/// \f[F = \left| 
	/// \begin{array}{cccc}
	/// 1&0&\Delta_T&0\\ 0&1&0&\Delta_T\\
	/// 0&0&1&0\\ 0&0&0&1\\
	/// \end{array}
	/// \right|
	/// \f]
	Matrix F;
	F <<1, 0, delta_T, 0,
		0, 1, 0, delta_T,
		0, 0, 1, 0,
		0, 0, 0, 1;

	/// \f$\hat{x}_{k|k-1}\f$ -- вектор состояния\n
	StateVector extr_x;

	/// Эволюция вектора состояния: 
	/// \f[\hat{x}_{k|k-1} = F \hat{x}_{k-1|k-1}\f]
	extr_x = F * m_x;

	/// Эволюция ошибки оценки состояния: 
	/// \f[ P_{k|k-1} = F P_{k-1|k-1} F^T + Q \f]
	Covariance extr_P = F * m_P * F.transpose() + m_Q;
	
	/// ## Коррекция
	/// \f$H\f$ -- матрица измерений
	/// \f[H = \left| 
	/// \begin{array}{cccc}
	/// 1&0&0&0\\ 0&1&0&0\\
	/// 0&0&0&0\\ 0&0&0&0\\
	/// \end{array}
	/// \right|
	/// \f]
	Matrix H;
	H <<1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 0, 0,
		0, 0, 0, 0;

	/// Рассогласование: \f[ \tilde{y}_k = z_k - H \hat{x}_{k|k-1} \f] 
	StateVector y, z;
	z << a_measure.getAsArray(), 0, 0;
	y = z - H * extr_x;

	/// Ковариция ошибки измерения: \f[ S_k = H P_{k|k-1} H^T + R \f] 
	Covariance S = H * extr_P * H.transpose() + m_R;

	/// Усиление: \f[ K_k = P_{k|k-1} H^T S^{-1} \f]
	Matrix K = extr_P * H.transpose() * S.inverse();

	/// Коррекция вектора состояния: 
	/// \f[ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k \tilde{y} \f]
	m_x = extr_x + K * y;

	// P(k|k) = (I - K * H) * P(k|k-1)
	m_P = (Matrix::Identity() - K * H) * extr_P;	

#ifdef _DEBUG_V
	std::cout 
		<< "X(" << m_time << ") = \n"
		<< m_x << "\n"
		<< "P(" << m_time << ") = \n"
		<< m_P << "\n";
#endif
}